/*
 * print.cpp
 *
 * Created: 29.7.2013 11:34:01
 *  Author: Ferda
 */ 

 #include "print.h"

 
 void printUINT32(WRITE_FUNC fn, uint32_t val, uint8_t base)
 {
	 if (val == 0) {											
		 fn('0');											
		 return;									
	 }									
	 
	 char buf[64];							
	 int8_t i = 0;									
	 
	 while (val > 0)										
	 {													
		 char mod = val % base;							
		 buf[i++] = mod < 10 ? '0' + mod : 'A' + mod - 10;	
		 val /= base;										
	 }													
	 
	 for (;--i >= 0;)									
		fn(buf[i]);
 }

void printINT32(WRITE_FUNC fn, int32_t val, uint8_t base)
{
	if (val < 0) {
		fn('-');
		val = -val;
	}

	printUINT32(fn, (uint32_t)val, base);
}
 

void printFloat(WRITE_FUNC fn, float number, uint8_t digits)
{
	// Handle negative numbers
	if (number < 0.0)
	{
		fn('-');
		number = -number;
	}

	// Round correctly so that print(1.999, 2) prints as "2.00"
	double rounding = 0.5;
	for (uint8_t i=0; i<digits; ++i)
	rounding /= 10.0;
	
	number += rounding;

	// Extract the integer part of the number and print it
	uint32_t int_part = (unsigned long)number;
	double remainder = number - (double)int_part;
	printUINT32(fn, int_part, 10);

	// Print the decimal point, but only if there are digits beyond
	if (digits > 0)
	fn('.');

	// Extract digits from the remainder one at a time
	while (digits-- > 0)
	{
		remainder *= 10.0;
		uint16_t toPrint = int(remainder);
		printUINT32(fn, (uint32_t)toPrint, 10);
		remainder -= toPrint;
	}
}

void printfn(WRITE_FUNC fn, const char * str, ...)
{
	va_list args;
	va_start(args, str);

	char c;
	while ( (c = *str++) != 0)
	{
		if (c != '%')
			fn(c);
		else
		{
			c = *str++;
			if (c == 'c')
				printINT32(fn, (int32_t)va_arg(args, int16_t), 10);
			else if (c == 'i')
				printINT32(fn, (int32_t)va_arg(args, int16_t), 10);
			else if (c == 'l')
				printINT32(fn, (int32_t)va_arg(args, int32_t), 10);
			else if (c == 'b')
				printUINT32(fn, (uint32_t)va_arg(args, uint16_t), 10);
			else if (c == 'w')
				printUINT32(fn, (uint32_t)va_arg(args, uint16_t), 10);
			else if (c == 'u')
				printUINT32(fn, (uint32_t)va_arg(args, uint32_t), 10);
			else if (c == 'f')
				printFloat(fn, (float)(va_arg(args, double)), 2);
			else if (c == 'd')
				printFloat(fn, (float)(va_arg(args, double)), 2);
		}
	}

	va_end(args);
}

/*
void printfnln(WRITE_FUNC fn, const char * str, ...) 
{ 
	va_list list; 
	va_start(list, str); 
	printfn(fn, str, list); 
	println(fn); 
}
*/

void printfn_P(WRITE_FUNC fn, const char *str, ...)
{
	va_list args;
	va_start(args, str);

	char c;
	while ( (c = pgm_read_byte_far(str++)) != 0)
	{
		if (c != '%')
			fn(c);
		else
		{
			c = pgm_read_byte_far(str++);
			if (c == 'c')
				printINT32(fn, (int32_t)va_arg(args, int16_t), 10);
			else if (c == 'i')
				printINT32(fn, (int32_t)va_arg(args, int16_t), 10);
			else if (c == 'l')
				printINT32(fn, (int32_t)va_arg(args, int32_t), 10);
			else if (c == 'b')
				printUINT32(fn, (uint32_t)va_arg(args, uint16_t), 10);
			else if (c == 'w')
				printUINT32(fn, (uint32_t)va_arg(args, uint16_t), 10);
			else if (c == 'u')
				printUINT32(fn, (uint32_t)va_arg(args, uint32_t), 10);
			else if (c == 'f')
				printFloat(fn, (float)(va_arg(args, double)), 2);
			else if (c == 'd')
				printFloat(fn, (float)(va_arg(args, double)), 2);
		}
	}


	va_end(args);
}

/*
void printfnln_P(WRITE_FUNC fn, const char *str, ...) 
{ 
	va_list list; 
	va_start(list, str); 
	printfn_P(fn, str, list); 
	println(fn); 
}
*/